COMMENT ⊗   VALID 00009 PAGES
C REC  PAGE   DESCRIPTION
C00001 00001
C00002 00002	.TITLE MOTOR
C00004 00003	PROGRAM INITIALIZATION 
C00007 00004	INITIALIZE FOR DRIVING MOTOR
C00010 00005	START TRANSFERING BUFFER
C00012 00006	CLOCK INTERRUPT ROUTINE, START BY SENDING OFF DAC VALUES,READ THE A/D
C00016 00007	WAIT TILL DAC AND A/D ROUTINE COMPLETED, THEN UPDATE VALUES FOR NEXT TICK
C00020 00008	DAC OUTPUT INTERRUPT ROUTINE
C00022 00009	DYNAMIC RESPONSE DATA STORAGE AREA
C00026 ENDMK
C⊗;
.TITLE MOTOR
.INSRT ALHEAD.PAL[AL,HE]
.=STRT11
.INSRT ARITH.PAL[AL,HE]

;DEFINITIONS:

ALARM==200	;TRAP VECTOR FOR PANIC BUTTON
TICK==100.	;LENGTH OF TICK IN CLOCK UNITS

CMDARR==77200	;ADDRESS OF COMMAND INPUT BUFFER FROM PDP10
JOINT ==77202	;JOINT NUMBER TO BE DRIVEN
DC    ==77204	;DC DRIVE COMPONENT
SINAMP==77206	;SINE AMPLITUDE
SINPER==77210	;SINE PERIOD
DURAT ==77212	;DURATION OF MOTION
POTTAC==77214	;FLAG INDICATING POT OR TACH TO BE READ
MASLOC==77216	;ADDRESS OF MASTER CHECK NUMBER FROM PDP10

MASTER==11711	;CHECK NUMBER FROM PDP10

DAC   :	174000	;DAC SAMPLE ADDRESS
DACOUT: 174001	;DAC VALUE REGISTER ADDRESS
BRAKES: 174002	;DAC CONTROL INFO ADDRESS
AD    : 174004	;A/D CONTROL INFO ADDRESS
ADVAL : 174006	;A/D SAMPLE ADDRESS
;PROGRAM INITIALIZATION 

.EVEN
START:	RESET
	MOV	#500,SP		;INITIALIZE STACK
	CLR	PS		;INITIALIZE PROCESSOR STATUS
      	CLR 	CLKCNT		;CLEAR CLOCK REGISTERS- TRAP RESTART
	CLR 	CLKSET
	CLR 	CLKS
	MOV	#PANIC,ALARM	;SET PANIC BUTTON TRAP TO PROPER ADDR
	MOV	#340,ALARM+2	
	MOV	#DACSER,DACVEC	;SET DAC INTERRUPT SERVICE ROUTINE
	MOV	#340,DACVEC+2
	MOV	#ADCSER,ADCVEC	;SET ADC INTERRUPT SERVICE ROUTINE
	MOV	#300,ADCVEC+2
	MOV	#CLKSER,CLKTRP	;SET CLOCK INTERRUPT SERVICE ROUTINE
	MOV	#100,CLKTRP+2
	LDFPS	#0		;SET SINGLE PRECISION FLOATING POINT
	CLR	MASLOC		;CLEAR MASTER NUMBER FROM PDP10
     	MOV 	#-1,CMDARR	;INDICATE READY TO RECEIVE COMMAND BLOCK FROM 10
REST:	MOV	#DR,DYNA	;SET POINTER TO START OF DYNAMIC RESPONSE DATA
				;  STORAGE AREA
	CLR	@DYNA 		;SET THE FIRST TIME TO ZERO
        MOV	#-1,WRAP	;FLAG INDICATING DR BUFFER WRAPS AROUND

;WAIT LOOP LOOKING AT COMMAND BLOCK FROM PDP10

WTLP:	TST	KBIS		;CHECK IF ANYONE HIT VT05 KEYBOARD
	BEQ	.+6
	JMP	RUG		;BACK TO RUG IF SOMEONE CALLED
      	TST	MASLOC		;CHECK IF COMMAND BLOCK RECEIVED FROM PDP10
	BEQ	WTLP		;GO LOOK AGAIN IF ZERO    
	CMP	#MASTER,MASLOC	;CHECK IF RIGHT MASTER NUMBER FROM PDP10
	BEQ	SMENUM
	MOV	#-2,CMDARR	;SET ERROR CODE INDICATING BAD NUMBER
	CLR	MASLOC		;CLEAR THE BAD MASTER NUMBER
	JMP	REST		;GO WAIT FOR ANOTHER COMMAND BLOCK
SMENUM:	CLR	MASLOC 
	MOV	#BLUE,R0	;GET POINTER TO BLUE ARM INTERFACE ADDRESSES
	TST	CMDARR     	;GET THE ARM NUMBER, EQUAL TO 1 IF BLUE ARM
	BNE	.+6		;SKIP IF BLUE
	MOV	#YELO,R0	;ELSE LOAD POINTER TO YELLOW ARM INTERFACE
	MOV	(R0),DAC   	;STORE INTERFACE ADDRESSES
	MOV	(R0)+,DACOUT
	INC	DACOUT
	MOV	(R0)+,BRAKES
	MOV	(R0)+,AD
	MOV	(R0)+,ADVAL
;INITIALIZE FOR DRIVING MOTOR

	MOV	#7,R0		;ZERO ALL DAC OUTPUT LEVELS
	MOV	#DACVAL,R1
	CLRB	(R1)+
	SOB	R0,.-2
	MOV	JOINT,R0	;GET JOINT NUMBER
	DEC	R0   		;JOINT INDEX, GOES FROM 0 TO 6
	MOV	R0,JOINT	;SAVE JOINT INDEX
	MOVB	DC,DACVAL(R0)	;SET INITIAL DC MOTOR DRIVE VALUE, SIN(0)=0
	MOV	DC,D1		;SAVE FIRST DRIVE VALUE
	LDCIF	SINAMP,AC0	;FLOAT THE AMPL. OF THE SINUSOID
	STF	AC0,AC4		;SAVE IT
	LDCIF	SINPER,AC1	;FLOAT THE PERIOD OF THE SINUSOID
	DIVF	THRE60,AC1	;DIVID BY 360 DEGREES
	STF	AC1,AC5
	MOVB	#10,@AD		;RESET ARM ERROR BITS
	BITB	#6,@AD		;CHECK IF MOTOR POWER NOT ON
	BEQ	RELBRK		;BRANCH IF POWER ON
	MOV	#-3,CMDARR	;ELSE SEND BACK ERROR MESSAGE
	JMP	REST		;EXIT
RELBRK:	MOVB	#20,@DAC	;RESET ARM INTERFACE
	MOV	#1,R1		;GET JOINT BRAKE BIT
	ASH	R0,R1	        
	BISB    TACH,R1       	;OR ON TACH FEEDBACK BITS
	MOV	R1,@BRAKES	;RELEASE JOINT BRAKE AND SET TACH FEEDBACK

;START CLOCK ROUTINE AND WAIT TILL DONE

	MOV	#1,RUN		;INDICATE JOINT RUNNING
	CLR	TIME		;KEEP TRACK OF PRESENT TIME
	MOV	#4,CLKSET	;START CLOCK ROUTINE TO DRIVE JOINT
	MOV	#111,CLKS
	MOV	#TICK,CLKSET	;SET CLOCK INTERVAL
CLKLP:	TST	RUN		;CHECK IF DRIVING COMPLETE
	BNE	CLKLP		;WAIT TILL DONE

;PUT THE DYNAMIC RESPONSE DATA IN THE PROPER ORDER

       	ADD     #2,DYNA
	TST	WRAP		;CHECK IF LESS THAN 500 WORDS
	BMI	RET		;IF WRAP STILL -1, NO WORK TO BE DONE
	BGT	DOESWR		;BRANCH IF THE SECOND BUFFER WRAPS AROUND
	MOV	#D2,FROM	;IF LESS THAN 1000, START TRANSFER FROM D2
	BR 	TRANS		;START TRANSFERING BUFFERS
DOESWR: MOV	DYNA,FROM	;FOR A WRAP AROUND, START TRANSFER FROM DYNA+2
TRANS:	MOV	#T2,TO		;MOVE SECOND TIME BUFFER TO T2
	MOV	#3,R2  		;COUNTER FOR TRANSFERING 3 BUFFERS
	MOV	#AG1,MAXA	;FIRST BUFFER ENDS AT AG1
;START TRANSFERING BUFFER

TRAN: 	MOV	FROM,R0		;PICK UP FIRST LOCATION TO BE TRANSFERED
	MOV	TO,R1  		;START MOVING FROM TO TO
TRA:	CMP	MAXA,R0   	;CHECK IF AT END OF BUFFER
	BNE	GOON		;SKIP IF STILL SOME LEFT
	SUB	#1000.,R0  	;POINT TO START OF BUFFER
GOON:	MOV	(R0)+,(R1)+	;TRANSFER A WORD
	CMP	DYNA,R0		;CHECK TRANSFER COMPLETED
	BNE	TRA		
	ADD	#2000.,MAXA	;POINT TO THE NEXT SET OF BUFFERS
	ADD	#2000.,TO
	ADD	#2000.,FROM
	ADD	#2000.,DYNA
	SOB	R2,TRAN		;DO ALL THREE BUFFERS

;INDICATE EXECUTION COMPLETED TO PDP10

RET:	CMP	TIME,#2		;CHECK IF JOINT STARTED OUT OF RANGE
	BGT	GODRUN		;BRANCH IF OK
	MOV	#-4,CMDARR	;ELSE SEND BACK ERROR MESSAGE
	JMP	REST		;EXIT
GODRUN:	MOV	#DR,R0		;GET THE ADDRESS OF THE DYNAMIC RESPONSE DATA
	ASR	R0		;GET WORD ADDRESS
	NEG	R0		;COMPLEMENT TO USE AS FLAG TO TELL PDP10 WE'RE DONE
	MOV	R0,CMDARR
	JMP	REST		;GO LOOP WAITING FOR ANOTHER COMMAND BLOCK
;CLOCK INTERRUPT ROUTINE, START BY SENDING OFF DAC VALUES,READ THE A/D

CLKSER:	MOV	R0,-(SP)	;SAVE REGISTERS
	MOV	R1,-(SP)
	MOV	R2,-(SP)
	MOV	R3,-(SP)
	MOV	#7,NUMJT	;SET POINTERS AND COUNTER TO REFRESH DACS
	MOV	#DACCHN,PDAC	
	MOV	#DACVAL,PVAL
	MOVB	#120,@DAC	;REFRESH DACS USING DAC INTERRUPT ROUTINE
	MOV	JOINT,R0	;GET JOINT NUMBER
	TST	POTTAC		;CHECK IF POT OR TACH TO BE READ
	BEQ	.+6		;SKIP IF POT
	ADD	#TACCHN-ADCCHN,R0	;ELSE ADJUST FOR TACH CHANNEL NUMBERS
	MOVB	ADCCHN(R0),R1	;GET ADC CHANNEL NUMBER
	SWAB	R1		;POSITION IN UPPER HALF WORD
	BIS	#101,R1		;OR "FEEL" BIT AND INTER. ENABLE ON TO CHAN #
	MOV	R1,@AD		;ENABLE A/D INTERRUPT ROUTINE

;SET NEXT POINTER FOR DYNAMIC RESPONSE DATA

	MOV	DYNA,R3		;GET POINTER TO DYNAMIC RESPONSE DATA
	ADD	#2,R3  		;POINT TO THE NEXT WORD OF THE TIME BUFFER
	CMP	#T2,R3  	;CHECK IF AT END OF FIRST BUFFER
	BNE	NOTT2		;BRANCH IF NOT END OF FIRST BUFFER
	MOV	#D2,R3    	;START PUTTING TIME DATA AFTER FIRST DRIVE BUFFER
	INC	WRAP		;INDICATE OVER 500 BLOCKS STORED
	BR 	SETT
NOTT2:	CMP	#AG1,R3   	;CHECK IF AT END OF SECOND TIME BUFFER
	BNE	SETT		;BRANCH IF NOT END
	MOV	#D2,R3  	;WRAP AROUND IN SECOND BUFFER
	INC	WRAP		;INDICATE WRAP AROUND

;INCREMENT TIME COUNT AND COMPUTE NEXT DRIVE VALUE 

SETT:	INC	TIME		;INCREMENT ELAPSED TIME COUNTER
	MOV	TIME,(R3) 	;STORE TIME IN DYNA. RESPONSE AREA
     	MOV	DC,2000.(R3) 	;SET NEW MOTOR DRIVE EQUAL TO THE DC COMPONENT
	TST	SINAMP		;CHECK IF AMPLITUDE OF SINUSOID ZERO
	BEQ	NOSIN
	TST	SINPER		;CHECK IF PERIOD OF SINUSOID ZERO
	BEQ	NOSIN
	CLR	R0		;CLEAR FOR DIVISION
	MOV	TIME,R1		;GET THE ELAPSED TIME
	DIV	SINPER,R0	;GET THE MOD OF THE SINE PERIOD AND EL.TIME
	LDCIF	R1,AC0		;FLOAT THE REMAINDER
	DIVF	AC5,AC0		;DIVID BY PERIOD AND CONVERT TO DEGREES
	JSR	PC,SNCOS	;COMPUTE SINE OF ANGLE
	MULF	AC4,AC0		;MULT. THE SINE BY THE AMPLITUDE 
	STCFI	AC0,R0		;GET THE INTEGER PART
	ADD	R0,2000.(R3) 	;ADD THE SINUSOID TO THE MOTOR DRIVE
;WAIT TILL DAC AND A/D ROUTINE COMPLETED, THEN UPDATE VALUES FOR NEXT TICK

NOSIN:	BITB	#100,@DAC	;CHECK IF DAC DONE YET
	BNE	CHKCLK		;BRANCH IF IT'S NOT
	BITB	#100,@AD	;CHECK IF A/D DONE YET
	BNE	CHKCLK		;BRANCH IF IT'S NOT
	CMP	DURAT,TIME	;CHECK IF MOTION COMPLETED
	BLT	JTSTOP		;BRANCH IF THIS IS THE END
	MOV	R3,DYNA		;ELSE UPDATE DYNAMIC RESPONSE DATA POINTER
	MOV	JOINT,R0	;GET PTR TO JOINTS DAC VALUE
	MOVB	2000.(R3),DACVAL(R0)	;SET ITS NEW DAC VALUE
	BR	CLKDNE		;EXIT CLEANLY
CHKCLK:	CMP	#10,CLKCNT	;CHECK IF LESS THAN 100 MICRO SECONDS LEFT
	BLT	NOSIN		;REPEAT CHECK IF MORE TIME LEFT

;STOP THE JOINT MOTION

JTSTOP:	BIC	#100,@DAC	;STOP ANY DAC OPERATIONS FROM TAKING PLACE
	CLR	CLKS		;STOP CLOCK
	MOV	#DACVAL,R0	;ZERO ALL DAC VALUES
	MOV	#7,R1		;SEVEN JOINTS IN ALL
	CLRB	(R0)+
	SOB	R1,.-2
	MOV	#7,NUMJT	;SET POINTERS AND COUNTER TO REFRESH DACS
	MOV	#DACCHN,PDAC	
	MOV	#DACVAL,PVAL
	MOVB	#120,@DAC	;ZERO DACS USING DAC INTERRUPT ROUTINE
	CLR	@BRAKES		;TURN ON ALL OF THE JOINT BRAKES
	MOV	#-1,(R3)  	;SET LAST TIME TO -1
	MOV	R3,DYNA		;SAVE PTR TO LAST DATA CELL USED
	CLR	RUN		;INDICATE JOINT SERVICING STOPPED

;EXIT CLOCK INTERRUPT ROUTINE CLEANLY

CLKDNE:	MOV	(SP)+,R3
	MOV	(SP)+,R2
	MOV	(SP)+,R1
	MOV	(SP)+,R0
	RTI
;DAC OUTPUT INTERRUPT ROUTINE

DACSER:	MOVB	@PDAC,@DAC	;SET DAC CHANNEL
	INC	PDAC
	DEC	NUMJT		;CHECK IF THIS IS THE LAST CHANNEL TO OUTPUT
	BLE	.+10
	BISB	#100,@DAC	;ENABLE INTERRUPTS AGAIN IF MORE JOINTS LEFT
	MOVB	@PVAL,@DACOUT	;SEND OFF DAC VALUE
	INC	PVAL
	RTI

;ANALOG TO DIGITAL CONVERTER INTERRUPT ROUTINE

ADCSER:	MOV	R0,-(SP)	;SAVE REGISTER
	TST	POTTAC		;CHECK IF TACHOMETER BEING READ
	BNE	SAVADC		;DON'T CHECK JOINT LIMITS IF SO
	MOV	JOINT,R0	;GET INDEX TO JOINT LIMIT DATA
	ASL	R0
	CMP	@ADVAL,MINJT(R0)	;CHECK IF JOINT IN RANGE
	BLT	JTOTRG		;BRANCH IF JOINT OUT OF RANGE
	CMP	@ADVAL,MAXJT(R0)	;ELSE REPEAT FOR MAXIMUM 
	BLE	SAVADC
JTOTRG:	CLR	DURAT		;INDICATE END OF JOINT SERVOING
SAVADC:	MOV	DYNA,R0		;GET PTR TO DYNAMIC RESPONSE ARRAY
	MOV	@ADVAL,4000.(R0)	;SAVE JOINT READING
	BIC	#101,@AD	;TURN OFF ADC INTERRUPTS AND ALLOW A/D TO TRACK
	MOV	(SP)+,R0
	RTI

;SEQUENCE TO FOLLOW IF PANIC BUTTON HIT

PANIC: 	CLR	DURAT		;INDICATE END OF JOINT SERVOING
	RTI
;DYNAMIC RESPONSE DATA STORAGE AREA

DR:	.BLKW	500.	;FIRST TIME BUFFER OF DYNAMIC RESPONSE DATA
T2:	.BLKW	500.	;SECOND TIME BUFFER
D1:	.BLKW	500.	;FIRST DRIVE BUFFER
D2:	.BLKW	500.	;SECOND DRIVE BUFFER
AG1:	.BLKW	500.	;FIRST JOINT ANGLE BUFFER
	.BLKW	500.	;SECOND JOINT ANGLE BUFFER
	.BLKW	1000.

;BLUE ARM INTERFACE ADDRESSES

BLUE:  	174000	;DAC SAMPLE ADDRESS
        174002	;DAC CONTROL INFO ADDRESS
        174004	;A/D CONTROL INFO ADDRESS
        174006	;A/D SAMPLE ADDRESS

;YELLOW ARM INTERFACE ADDRESSES

YELO:  	174000	;DAC SAMPLE ADDRESS
        174002	;DAC CONTROL INFO ADDRESS
        174004	;A/D CONTROL INFO ADDRESS
        174006	;A/D SAMPLE ADDRESS

;DAC/ADC DATA

ADCCHN:	.BYTE	21,14,15, 2, 3, 4, 5, 0
TACCHN:	.BYTE	20,10,11, 6, 7, 0, 0, 0
DACCHN:	.BYTE	 1, 2, 3, 4, 5, 6, 7, 0
DACVAL:	.BYTE	 0, 0, 0, 0, 0, 0, 0, 0
.EVEN

;MINIMUM AND MAXIMUM JOINT ANGLE READINGS PERMITTED FOR JOINT IN WORKING RANGE

MINJT:	-7777
	-7777
	-7777
	-7777
	-7777
	-7777
	-7777 
MAXJT:	7777
	7777
	7777 
	7777
	7777
	7777
	7777

;LOCAL VARIABLES

WRAP:   -1  	;-1: LESS THAN 500 WORDS OF DR, 0: LESS THAN 1000 WORDS OF DR
		;  GT 0: MORE THAN 1000 WORDS OF DR, DATA WRAPS AROUND 2ND 500
DYNA:	DR	;POINTER TO DYNAMIC RESPONSE DATA ARRAY
RUN:	0	;FLAG INDICATING IF JOINT IN MOTION
NUMJT:	7	;NUMBER OF DACS TO REFRESH
PDAC:	0	;PTR TO PRESENT DAC BEING SERVICE
PVAL:	0	;PTR TO PRESENT DAC VALUE TO WRITE
TACH:	0	;TACH FEEDBACK BITS
THRE60:	.WORD	42264,0		;360.0
TO:	0
FROM:	0
MAXA:	0
TIME:	0	;CURRENT TIME INTO MOTION

DBUF:	.BLKW	300.

.END START